/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
 * FILENAME:
 * ukfTrace.cxx
 *
 * PURPOSE:
 * This interface block runs a UKF to localize WiFi nodes and plans a path to
 * improve the estimate.
 *
 * CREATED:
 * 08/2011 by Maciej Stachura
*/ 

#include <iostream>

#include "ukfTrace.h"
#include "utilities.h"
#include "color.h"
#include "planner.h"

using namespace std;

/*<---External Global Variables---->*/
extern bool verbose;
extern bool logging;
extern String logDir;

/*<---Global Variables---->*/
int myVerbosity = 0;

/*<---Class functions ---->*/
UKFTrace::UKFTrace(String name) : ThreadedPipe(name)
{
	cout << GREEN << "---------->>>>>      "YELLOW"TRACE"GREEN"      <<<<<----------"  << ATTROFF<< endl;
	buf_ptr   = 0;
	linkCount = 0;
	smoothing = 1;

  datum.lat = 40.14838;
  datum.lon = -105.24;
  datum.alt = 1650.0;
  my_pos.x = 0.0;
  my_pos.y = 0.0;
  my_pos.z = 0.0;

	for(int i=0; i < MAX_RSSI_CLIENTS; i++ ) {
		for(int j=0; j < BUTTER_NZEROS+1; j++ ) 
			linkStat[i].xv[j] = NAN;
		for(int j=0; j < BUTTER_NPOLES+1; j++ ) 
			linkStat[i].yv[j] = NAN;
	}
  plan_param.N = 3;
  u_star = new double[plan_param.N];
  u_init = new double[plan_param.N];
  plan_param.inrgstddev = new double[plan_param.N];
}

UKFTrace::~UKFTrace()
{
	trace_destroy(trace);
	trace_destroy_packet(packet);
  delete [] u_star;
  delete [] u_init;
  delete [] plan_param.inrgstddev;
}

bool UKFTrace::init(ShmStruct * shmStructPtr)
{
	ThreadedPipe::init(shmStructPtr);
	last_rssi_tx.stamp();

	Client * me = shmPtr->clients.getClient(0);
	shmPtr->clients.subscribe(me, CMD_TELEM);
	shmPtr->clients.subscribe(me, CMD_FP);

	/*--------- log ----------*/
	if(logging) {
		log_name = generteNumberedFile( logDir, "rssi_trace_");
    ukf_name = generteNumberedFile( logDir, "ukf_trace_");

    // Write the header
    ukffile=fopen(ukf_name.c_str(), "a");
    fprintf(ukffile,"%s\n", "%mode(1) time(1) X_s(4) mac(6) signal(1) X(4) P(16) u(1)");
    if(ukffile != NULL) {
      fflush(ukffile);
      fclose(ukffile);
    }

	}

	/* Create the trace */
	trace = trace_create("int:man0");
	if (trace_is_err(trace)) {
		trace_perror(trace, "trace_create");
		return false;
	}

	/* Starting the trace */
	if (trace_start(trace) != 0) {
		trace_perror(trace, "trace_start");
		return false;
	}

	packet = trace_create_packet();

  // UKF init
  reset_filt();
  ukf_started = FALSE;
	srand(time(NULL));

  // Planner init
  plan_param.dt = RSSI_PERIOD;
  plan_param.dyn = &ua_dynamics;
  plan_param.H = &get_H;
  for (size_t i=0;i<plan_param.N;i++){
    u_star[i] = 0.0;
    u_init[i] = 0.0;
    plan_param.inrgstddev[i] = 0.1;
  }
  plan_param.x.SetSize(4,1); plan_param.x.Null();
  plan_param.P.SetSize(2,2); plan_param.P.Null();
  plan_param.Q.SetSize(2,2);
  plan_param.Q(0,0) = filt_snr.param.Q(0,0);
  plan_param.Q(0,1) = filt_snr.param.Q(0,1);
  plan_param.Q(1,0) = filt_snr.param.Q(1,0);
  plan_param.Q(1,1) = filt_snr.param.Q(1,1);
  plan_param.R.SetSize(1,1); plan_param.R = filt_snr.param.R;
  plan_param.Xs.SetSize(3,1); plan_param.Xs.Null();
  plan_param.TAS = 0.0;


	return true;
}

void UKFTrace::my_per_packet(libtrace_packet_t *packet)
{
	/* Packet data */
	uint32_t remaining = 0;

	int val;
	int linkInd = -1;

	// get the src mac and see if we have that address stored
	// in our link list
	uint8_t *mac = trace_get_source_mac(packet);
	if(mac) {
	//if(mac && mac[4] == 0x62 && mac[5] == 0xc2) {
		// now we need to look up source make in linkStat
		unsigned int ind=0; 
		for( ; ind < linkCount && ind < MAX_RSSI_CLIENTS; ind++) {
			int i;
			for(i=0; i<6; i++){
				if( mac[i] != linkStat[ind].mac[i] )
					break;
			}
			// found link in list
			if( i==6 ) {
				linkInd = ind;
				break;
			}
		}
		// found new link
		if( ind == linkCount && linkCount < MAX_RSSI_CLIENTS ) {
			linkInd = ind;	
			linkCount++;

			for(int i=0; i<6; i++)
				linkStat[linkInd].mac[i] = mac[i];

		} else if( linkCount == MAX_RSSI_CLIENTS && ind == MAX_RSSI_CLIENTS) {
			if(verbose)
				printf("!!! UKFTrace:: reached maximum number of clients to track\n");
			return;
		}
	}
	else return; // if no mac, return

	//-- packet link --//
	libtrace_linktype_t linktype = trace_get_link_type(packet);
	switch(linktype) {
		case TRACE_TYPE_80211_RADIO:
			if(myVerbosity) printf(" Radiotap + 802.11");
			break;
		default:
			printf(" No radiotap information\n");
			return;
	}

	// get pointer to the start of the link part of the packet
	uint8_t *buf;
	buf = (uint8_t*)trace_get_packet_buffer (packet, &linktype, &remaining);

	// rssi - for atheros rss->snr
	int8_t signal = 0;
	val = trace_get_wireless_signal_strength_dbm(buf, linktype, &signal);
  if(val) {
		if( smoothing )
			linkStat[linkInd].signal = butter_filter_5((float)signal, linkStat[linkInd].xv, linkStat[linkInd].yv);
    else{
			linkStat[linkInd].signal += signal;
      linkStat[linkInd].sig_count++;}


		if(myVerbosity) 
			printf(" sig=%i dBm avg=%f dBm", signal, linkStat[linkInd].signal);
	}



	if(myVerbosity) {
		printf("\n");
		fflush(stdout);
	}

}

void UKFTrace::arp() 
{
	FILE * proc = fopen("/proc/net/arp","r");

	if( !proc ) {
		cout << "!!! could not open /proc/net/arp" << endl;
		return;
	}
	char ipStr[16];
	char macStr[18];

	char tmp[256];
	fgets(tmp, 255, proc);
	while (!feof(proc)) 
	{
		int val = fscanf(proc, "%15s %*s %*s %17s %*s %*s", ipStr, macStr);
		if(val == 2) {
			uint8_t *mac = trace_ether_aton(macStr, NULL);
			for( int i=0; i < MAX_RSSI_CLIENTS ; i++ )  {
				int sum = 0;
				int zeros = 0;
				for( int j=0; j < 6; j++ ) {
					if( linkStat[i].mac[j] == mac[j] )
						sum++;
					if( mac[j] == 0 )
						zeros++;
				}
				if( sum == 6 && zeros < 6 ) {
					int ip[4];
					sscanf(ipStr,"%i.%i.%i.%i",&ip[0], &ip[1], &ip[2], &ip[3]);
					for( int j=0; j <  4; j++ )
						linkStat[i].ip[j] = ip[j];
					i=MAX_RSSI_CLIENTS;
				}
			}
		} else if( val != -1 )
			cout << "-- bad format val=" << val << endl;
	}

	if( proc )
		fclose(proc);
}

void UKFTrace::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;									 // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);													// zero read fd set
	FD_ZERO(&writeFDs);													// zero write fd set

	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);
	maxFD = fifo_FDs[INBOARD_RX];
	maxFD = (fifo_FDs[INBOARD_TX] > maxFD ? fifo_FDs[INBOARD_TX] : maxFD);

	maxFD = 0;

	// get ip address of nodes from their mac
	// No need with AODV
	arp() ;

	obj = trace_event(trace, packet);

	switch(obj.type) {
		case TRACE_EVENT_IOWAIT:
			FD_SET(obj.fd, &readFDs);
			maxFD = (obj.fd > maxFD ? obj.fd : maxFD);
			break;

			// We've got a packet! 
		case TRACE_EVENT_PACKET:
			// Check for error first 
			if (obj.size == -1)
				printf("!!! trace_event error\n");
			else
				my_per_packet(packet);
			break;

		default:
			break;
	}

	// do select to wait for wireless link information
	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("UKFTrace::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(obj.fd, &readFDs)) {
			// a packet is waiting, but don't process it till the next call
			// to trace event. Process packet is switch statement above
			;
		} 

		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			readFifo(INBOARD_RX);
		}
	}

	// rssi loop run time --
	//   send the known link values to those who have requested it 
	//   and log the data
	currentStamp.stamp();
	double diff = (currentStamp - last_rssi_tx) / SEC2MICRO;

  // Check to reset the UKF
  if( diff >= (RSSI_PERIOD) ) 
  {
    Client * me = shmPtr->clients.getClient(0);
    if(!ukf_started && me->telemetry.navigation.APon){
      ukf_started = TRUE;
      reset_filt();
    }else if(ukf_started && !me->telemetry.navigation.APon)
      ukf_started = FALSE;


    // mark timestamp for runloop
    last_rssi_tx = currentStamp;

    // update the ua position in the local coordinates
    my_pos = lla2local(me->telemetry.gps.pos, datum);
    gps_track = me->telemetry.gps.track;
    filt_snr.param.dataF[XS_X] = my_pos.x;
    filt_snr.param.dataF[XS_Y] = my_pos.y;
    filt_snr.param.dataF[XS_ALT] = my_pos.z;

		if(logging){
			logfile=fopen(log_name.c_str(), "a");
			ukffile=fopen(ukf_name.c_str(), "a");}

		for( unsigned int i=0; i < linkCount; i++ ) {

			currentStamp.stamp();
      if(!smoothing){
        if(linkStat[i].sig_count == 0)
          linkStat[i].signal = -1.0;
        else
          linkStat[i].signal /= linkStat[i].sig_count;}

      if(linkStat[i].mac[4] == 0x8C && linkStat[i].mac[5] == 0x75) {// UKF time!
        if(ukf_started){
          Matrix z(1,1);
          if(RSSI_SIM){
            Matrix x_sim(4,1);
            x_sim(0,0) = 0.0;
            x_sim(1,0) = 0.0;
            x_sim(2,0) = 0.00393739104314895;
            x_sim(3,0) = 0.790722170955444;
            z = g_snr(x_sim, filt_snr.param);
            z(0,0) += normal(0.0, 2.7042);
          }else
            z(0,0) = linkStat[i].signal;

          // Estimate state
          filt_snr.time_update();
          filt_snr.meas_update(z);
          // Command UA
          // Waypoint Control
          /*GPSLLA_t waypt_cmd;
          waypt_cmd.lat = datum.lat+M_TO_LAT(filt_snr.x(1,0));
          waypt_cmd.lon = datum.lon+M_TO_LON(filt_snr.x(0,0),waypt_cmd.lat);

          waypt_cmd.alt = WAYPT_ALT;
          update_waypt(waypt_cmd);*/

          // Turn Rate control
          // update_turn_rate(-10*DEG_2_RAD, me->telemetry.airdata.tas);
        }

        if(logging && ukffile != NULL) {
          update_ukf_str(i);
          fprintf(ukffile,"%s\n", ukfStr);
        }
      }

      // logging
      if(logging && logfile != NULL) {
        update_output_str(i);
        fprintf(logfile,"%s\n", outStr);
      }

      if(!smoothing){
        linkStat[i].signal = 0.0;
        linkStat[i].sig_count =  0;
      }
    }
    // Control the UA
    plan_param.x = filt_snr.x;
    plan_param.P(0,0) = filt_snr.P(0,0); plan_param.P(0,1) = filt_snr.P(0,1);
    plan_param.P(1,0) = filt_snr.P(1,0); plan_param.P(1,1) = filt_snr.P(1,1);
    plan_param.Xs(0,0) = filt_snr.param.dataF[XS_X];
    plan_param.Xs(1,0) = filt_snr.param.dataF[XS_Y];
    plan_param.Xs(2,0) = gps_track*DEG_2_RAD;
    plan_param.TAS = me->telemetry.airdata.tas;
    planner<planner_t> plan(&J_nocomm, plan_param);
    plan.init();
    for (size_t i=0;i<plan_param.N-1;i++)
      u_init[i] = u_star[i+1];

    u_star = plan.update(u_init);
    update_turn_rate(u_star[0], me->telemetry.airdata.tas);

    //-- flush and close log file
		if(logging && logfile != NULL) {
			fflush(logfile);
			fclose(logfile);
		}
		if(logging && ukffile != NULL) {
			fflush(ukffile);
			fclose(ukffile);
		}
	}
}

void UKFTrace::toggleSmoothing(int setVal)
{
	if( setVal == 0 || setVal == 1)
		smoothing = setVal;
	else
		smoothing ^= 1;
}

void UKFTrace::update_output_str(int clientInd)
{
	int n=0;

	currentStamp.stamp();

	n += sprintf(outStr + n,"%.1f ",currentStamp.time());
	n += sprintf(outStr + n,"%.1f %.1f %.1f %.1f ",my_pos.x, my_pos.y, my_pos.z, gps_track);
	n += sprintf(outStr + n,"%s",trace_ether_ntoa(linkStat[clientInd].mac, NULL));
	n += sprintf(outStr + n," %s",inet_ntoa( *((in_addr *)linkStat[clientInd].ip)) );
	n += sprintf(outStr + n," %.1f",linkStat[clientInd].signal);
}

void UKFTrace::update_ukf_str(int clientInd)
{
	int n=0;

	currentStamp.stamp();

	n += sprintf(ukfStr + n,"%i ",ukf_started);
	n += sprintf(ukfStr + n,"%.1f ",currentStamp.time());
	n += sprintf(ukfStr + n,"%.1f %.1f %.1f %.1f ",my_pos.x, my_pos.y, my_pos.z, gps_track);
	n += sprintf(ukfStr + n,"%s",trace_ether_ntoa(linkStat[clientInd].mac, NULL));
	n += sprintf(ukfStr + n," %.1f",linkStat[clientInd].signal);
	n += sprintf(ukfStr + n," %f %f %f %f",filt_snr.x(0,0),filt_snr.x(1,0),filt_snr.x(2,0),filt_snr.x(3,0));
	n += sprintf(ukfStr + n," %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f",filt_snr.P(0,0),filt_snr.P(0,1),filt_snr.P(0,2),filt_snr.P(0,3),filt_snr.P(1,0),filt_snr.P(1,1),filt_snr.P(1,2),filt_snr.P(1,3),filt_snr.P(2,0),filt_snr.P(2,1),filt_snr.P(2,2),filt_snr.P(2,3),filt_snr.P(3,0),filt_snr.P(3,1),filt_snr.P(3,2),filt_snr.P(3,3));
	n += sprintf(ukfStr + n," %f ",u_star[0]);
}

void UKFTrace::reset_filt()
{
  filt_snr.param.alpha = 1.0;

  filt_snr.param.R.Null(1,1);
  filt_snr.param.R(0,0) = 9.0;

  filt_snr.param.Q.Null(4,4);
  filt_snr.param.Q(0,0) = 1e-2;
  filt_snr.param.Q(1,1) = 1e-2;
  filt_snr.param.Q(2,2) = 5e-10;
  filt_snr.param.Q(3,3) = 5e-9;

  filt_snr.x.Null(4,1);
  filt_snr.x(0,0) = 100.0; 
  filt_snr.x(1,0) = 100.0; 
  filt_snr.x(2,0) = 0.003; 
  filt_snr.x(3,0) = 0.7;

  filt_snr.P.Null(4,4);
  filt_snr.P(0,0) = 2e4;
  filt_snr.P(1,1) = 2e4;
  filt_snr.P(2,2) = 1e-7;
  filt_snr.P(3,3) = 2e-4;

  filt_snr.f = &f_stat2;
  filt_snr.g = &g_snr;

  filt_snr.param.dataF[TAR_ALT] = 0.0;
}

void UKFTrace::update_turn_rate(float turn_rate, float tas) {
	Client * me = shmPtr->clients.getClient(0);
	GotoBank_t * bank_cmd = (GotoBank_t*)CMD_DATAPTR(fifo_msg.data);

  float bank = atan2(turn_rate*tas, 9.8);
  if(bank > MAX_BANK) bank = MAX_BANK;
  if(bank < -MAX_BANK) bank = -MAX_BANK;

  bank_cmd->value = bank;
  bank_cmd->control = 1;

	CMD_CMD(fifo_msg.data)  = CMD_BANK_LOOP;
	CMD_NUM(fifo_msg.data)  = sizeof(GotoBank_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	// send the command to myself, for the thread that handles piccolo interface
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
	writeFifo(INBOARD_TX);
}

void UKFTrace::update_waypt(GPSLLA_t orbit_point) {

	Client * me = shmPtr->clients.getClient(0);

	/*----------[ using the autopilot to orbit ]----------*/
	me->telemetry.navigation.OBmode = OB_ORBIT; 

	Waypt_t * waypoint = (Waypt_t*)CMD_DATAPTR(fifo_msg.data);
	waypoint[0].number   = WAYPT_NUM;
	waypoint[0].pos.lat  = orbit_point.lat;
	waypoint[0].pos.lon  = orbit_point.lon;
	waypoint[0].pos.alt  = WAYPT_ALT;
	waypoint[0].orbitRad = ORBIT_RADIUS;
	waypoint[0].next     = WAYPT_NUM-1;

	waypoint[1].number   = WAYPT_NUM - 1;
	waypoint[1].pos.lat  = me->telemetry.gps.pos.lat;
	waypoint[1].pos.lon  = me->telemetry.gps.pos.lon;
	waypoint[1].pos.alt  = WAYPT_ALT;
	waypoint[1].orbitRad = 0;
	waypoint[1].next     = WAYPT_NUM;

	CMD_CMD(fifo_msg.data)  = CMD_FP;
	CMD_NUM(fifo_msg.data)  = 2*sizeof(Waypt_t);
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	// send the command to myself, for the thread that handles piccolo interface
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	fifo_msg.src.s_addr = shmPtr->clients.getAddr(me).s_addr;
	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(me).s_addr;
	writeFifo(INBOARD_TX);
}

// Utility Function for 1 UA, 1 target, and no comm. constraints
double J_nocomm(double* u, planner_t param){
  double cost = 0.0;
  Matrix Xs = param.Xs;
  Matrix H(1,2), Yk = param.P.Inv();
  for(size_t i = 0; i < param.N; i++){
    Xs = param.dyn(u[i], Xs, param.TAS, param.dt);
    H = param.H(Xs, param.x);
    Yk = (param.Q + Yk.Inv()).Inv() + ~H*H/param.R(0,0);    
    cost -= Yk.Trace();
  }
  return cost;
}

// Non-holonomic vehicle model of the UA
Matrix ua_dynamics(double u, Matrix Xs, double TAS, double dt){
  Matrix Xs_new(3,1);
  Xs_new(2,0) = Xs(2,0) + dt*u;
  if(fabs(u) < 1e-10){
    Xs_new(0,0) = Xs(0,0) + TAS*dt*cos(Xs_new(2,0));// + dt*W(0,0);
    Xs_new(1,0) = Xs(1,0) + TAS*dt*sin(Xs_new(2,0));// + dt*W(1,0);
  }
  else{
    Xs_new(0,0) = Xs(0,0) + TAS/u * (sin(Xs_new(2,0)) - sin(Xs(2,0)));// + dt*W(0,0);
    Xs_new(1,0) = Xs(1,0) - TAS/u * (cos(Xs_new(2,0)) - cos(Xs(2,0)));// + dt*W(1,0);
  }
  return Xs_new;
}

// Compute the H matrix for the information filter
Matrix get_H(Matrix Xs, Matrix X){
  Matrix H(1,2);

  double r  = sqrt( (Xs(0,0)-X(0,0))*(Xs(0,0)-X(0,0)) + (Xs(1,0)-X(1,0))*(Xs(1,0)-X(1,0)) );

  H(0,0) = -10*X(3,0)*(X(0,0) - Xs(0,0)) / (r*r*log(10));
  H(0,1) = -10*X(3,0)*(X(1,0) - Xs(1,0)) / (r*r*log(10));
  return H;
}

// Stationary target state transition function.
Matrix f_stat2(Matrix x, FiltData_t param){
  return x;
}
// Stargazer Measurement funciton.
Matrix g_snr(Matrix x, FiltData_t param){
  Matrix out(1,1);
  float range = sqrt( pow((param.dataF[XS_X] - x(0,0)), 2) + 
                      pow((param.dataF[XS_Y] - x(1,0)), 2) +
                      pow((param.dataF[XS_ALT] - param.dataF[TAR_ALT]),2) );

  out(0,0) = 20*log10(x(2,0) / pow(range,x(3,0)));
  return out;
}

double uniform(){
	return ((double)rand() / (double)(RAND_MAX));
}

double normal(double mean, double std){
	return std*sqrt( -2*log( uniform() ) ) * cos(2*M_PI*uniform()) + mean;
}
